#!/usr/bin/env python
# coding: utf-8

def take(n, iterable):
    "Return first n items of the iterable as a list"
    from itertools import islice
    return list(islice(iterable, n))

class NotchAdapt(object):
    """
    Implementación de Notch adaptativo para procesamiento en tiempo real.
    Procesa bloques de longitud block_size y devuelve la salida.

    Uso en tiempo real:

    >>> n = NotchAdapt(6, 3, 0.1) # la entrada en bloques de a 6 y 3 coeficientes y step de 0.1

    >>> n([2,4,6,8,10,12], [1,2,3,4,5,6]) # entrada y referencia
    [0, 0.0, 2.0, 0.0, 0.79999999999999893, -3.3599999999999959] # salida

    le metemos otro bloque
    >>> n([2,4,6,8,10,12], [1,2,3,4,5,6])
    [21.295999999999982, -60.508799999999937, 49.99231999999995,
    -16.209600000000002, 49.174208, -237.32991360000005]
    """
    def __init__(self, block_size, filter_length, step_size=0.1):
        self._M = filter_length
        self._L = filter_length - 1
        self._mu = step_size
        self._entrada_old = [0] * self._L
        self._ref_old = [0] * self._L
        self.block_size = block_size
        self._wn = [0] * filter_length

    def __call__(self, entrada, referencia):
        out, wn = self._adapt_with_indices(entrada, self._entrada_old, referencia, self._ref_old, self._wn, self.block_size, self._M, self._mu)
        self._entrada_old = entrada[-self._L: ]
        self._ref_old = referencia[-self._L: ]
        self._wn = wn
        return out

    def _adapt_with_indices(self, entrada, entrada_old, ref, ref_old, wn, block_size, filter_length, mu = 0.1):
        """
        ref = bloque de vector de referencias
        ref_old = M-1 ultimos valores del anterior vector de referncia
        wn = coeficientes
        mu = step
        """
        M = filter_length
        L = filter_length - 1
        B = block_size
        ref_estimado = [0]*B
        out = [0] * B
        for k in xrange(0, B):
            acum = 0
            r = [0] * M
            for n, i in zip(range(M), range(k, k + M)):
                if i < L:
                    r[n] = ref_old[i]
                else:
                    r[n] = ref[i-L]
                acum += r[n] * wn[n]
            if k < L:
                out[k] = entrada_old[k] - acum
            else:
                out[k] = entrada[k-L]-acum
            for n in range(len(wn)):
                wn[n] += mu * r[n] * out[k]
            #print wn
        return out, wn

    def procesar_offline(self, entrada, referencia):
        """
        Procesa toda una entrada con una senial como referencia y devuelve la salida
        del procesamiento.
        """
        ientrada, ireferencia  = iter(entrada), iter(referencia)
        B = self.block_size
        salida = []
        while ientrada:
            try:
                salida += self.__call__(take(B, ientrada), take(B, ireferencia))
            except IndexError:
                break
        return salida

def test_2_senoides():
    import math
    # Creo 2 seniales senoidales
    senial_pura = [math.sin(2* math.pi * 500.0/8192 * n ) for n in range(10000)]
    referencia = [0.5 * math.sin(2* math.pi * 100.0/8192 * n ) for n in range(10000)]

    entrada = [senial_pura[n] + referencia[n] for n in range(10000)]


    n1 = NotchAdapt(block_size=50, filter_length=5, step_size= 0.01)    salida = n1.procesar_offline(entrada, referencia)

    import pylab
    import scipy as sp
    pylab.plot(sp.absolute(sp.fft(entrada[2500:])[:1000]),"r")
    pylab.plot(sp.absolute(sp.fft(salida[2500:])[:1000]),"b")
    pylab.show()
